{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "ce0f962c",
   "metadata": {},
   "source": [
    "# Test orbit file (LED) reading and Hermite interpolation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "d3eacd57",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import pandas as pd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "6e6eb96f",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import matplotlib\n",
    "from mpl_toolkits.mplot3d import Axes3D\n",
    "from matplotlib.lines import Line2D\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "f7e1a13b-f112-42f5-a4f8-f3b03ec03f13",
   "metadata": {},
   "outputs": [],
   "source": [
    "from pygmtsar import PRM\n",
    "from scipy.interpolate import CubicHermiteSpline"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5fe821c9-70f8-45b7-98ef-1df34b1055ba",
   "metadata": {},
   "source": [
    "### Load Parameters and Orbits"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "dbd28850-a46e-4fcb-be3e-0e9e1774eaab",
   "metadata": {},
   "outputs": [],
   "source": [
    "prm_ref = PRM.from_file('S1_20201222_ALL_F2.PRM')\n",
    "prm_rep = PRM.from_file('S1_20210103_ALL_F2.PRM')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "b9f84ecd-4971-4ae4-add6-57eae9599e22",
   "metadata": {},
   "outputs": [],
   "source": [
    "orb_ref = prm_ref.read_LED()\n",
    "orb_rep = prm_rep.read_LED()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e942b630-b2d2-41b5-a25b-ebc7e0c9e98c",
   "metadata": {},
   "source": [
    "### Reference"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "58a8925c-e6a6-48e9-b867-0438a1323043",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-1.5273690223693848e-07, -1.6391277313232422e-07, -1.7881393432617188e-07)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t_ref = prm_ref.get_seconds()\n",
    "t_ref = [t_ref[0], np.sum(t_ref)/2, t_ref[-1]]\n",
    "t_ref[0] - 30811049.791369, t_ref[1] - 30811051.287814, t_ref[2] - 30811052.784259\n",
    "# % ./SAT_baseline.sh\n",
    "# using command line\n",
    "# SC_identity = 10 \n",
    "# ......master LED file S1_20201222_ALL_F2.LED \n",
    "# .........aligned LED file S1_20210103_ALL_F2.LED \n",
    "# \treference orbit times: 30811049.791369, 30811052.784259, 30811051.287814\n",
    "# \trepeat orbit times: 225449.207167, 225452.200057, 225450.703612"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "98143f62-b53f-4756-b066-e0a353cd9d48",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "2.9928899742662907"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t_ref[-1] - t_ref[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "9bf39099-2fa5-47aa-8fe7-5bd86012a246",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{'nd': '281', 'iy': '2020', 'id': '356', 'isec': '51252.000000', 'idsec': '10.000'}\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>iy</th>\n",
       "      <th>id</th>\n",
       "      <th>isec</th>\n",
       "      <th>px</th>\n",
       "      <th>py</th>\n",
       "      <th>pz</th>\n",
       "      <th>vx</th>\n",
       "      <th>vy</th>\n",
       "      <th>vz</th>\n",
       "      <th>clock</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>51252.0</td>\n",
       "      <td>2.126633e+06</td>\n",
       "      <td>4.552170e+06</td>\n",
       "      <td>-4.989856e+06</td>\n",
       "      <td>3884.019377</td>\n",
       "      <td>3895.866320</td>\n",
       "      <td>5213.514095</td>\n",
       "      <td>30809652.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>51262.0</td>\n",
       "      <td>2.165382e+06</td>\n",
       "      <td>4.590845e+06</td>\n",
       "      <td>-4.937442e+06</td>\n",
       "      <td>3865.729372</td>\n",
       "      <td>3839.235712</td>\n",
       "      <td>5269.275977</td>\n",
       "      <td>30809662.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>51272.0</td>\n",
       "      <td>2.203946e+06</td>\n",
       "      <td>4.628953e+06</td>\n",
       "      <td>-4.884473e+06</td>\n",
       "      <td>3846.923435</td>\n",
       "      <td>3782.199871</td>\n",
       "      <td>5324.450320</td>\n",
       "      <td>30809672.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>51282.0</td>\n",
       "      <td>2.242319e+06</td>\n",
       "      <td>4.666488e+06</td>\n",
       "      <td>-4.830955e+06</td>\n",
       "      <td>3827.603002</td>\n",
       "      <td>3724.765828</td>\n",
       "      <td>5379.030847</td>\n",
       "      <td>30809682.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>51292.0</td>\n",
       "      <td>2.280496e+06</td>\n",
       "      <td>4.703447e+06</td>\n",
       "      <td>-4.776894e+06</td>\n",
       "      <td>3807.769575</td>\n",
       "      <td>3666.940659</td>\n",
       "      <td>5433.011344</td>\n",
       "      <td>30809692.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>...</th>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>276</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>54012.0</td>\n",
       "      <td>-2.069327e+06</td>\n",
       "      <td>-3.293189e+06</td>\n",
       "      <td>5.901384e+06</td>\n",
       "      <td>-5109.703461</td>\n",
       "      <td>-3947.281735</td>\n",
       "      <td>-3985.399903</td>\n",
       "      <td>30812412.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>277</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>54022.0</td>\n",
       "      <td>-2.120336e+06</td>\n",
       "      <td>-3.332440e+06</td>\n",
       "      <td>5.861198e+06</td>\n",
       "      <td>-5091.966582</td>\n",
       "      <td>-3902.738810</td>\n",
       "      <td>-4051.757205</td>\n",
       "      <td>30812422.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>278</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>54032.0</td>\n",
       "      <td>-2.171164e+06</td>\n",
       "      <td>-3.371243e+06</td>\n",
       "      <td>5.820351e+06</td>\n",
       "      <td>-5073.593365</td>\n",
       "      <td>-3857.783983</td>\n",
       "      <td>-4117.659359</td>\n",
       "      <td>30812432.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>279</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>54042.0</td>\n",
       "      <td>-2.221806e+06</td>\n",
       "      <td>-3.409594e+06</td>\n",
       "      <td>5.778847e+06</td>\n",
       "      <td>-5054.585233</td>\n",
       "      <td>-3812.423186</td>\n",
       "      <td>-4183.098903</td>\n",
       "      <td>30812442.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>280</th>\n",
       "      <td>2020</td>\n",
       "      <td>356</td>\n",
       "      <td>54052.0</td>\n",
       "      <td>-2.272254e+06</td>\n",
       "      <td>-3.447490e+06</td>\n",
       "      <td>5.736690e+06</td>\n",
       "      <td>-5034.943691</td>\n",
       "      <td>-3766.662398</td>\n",
       "      <td>-4248.068426</td>\n",
       "      <td>30812452.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>281 rows × 10 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "       iy   id     isec            px            py            pz  \\\n",
       "0    2020  356  51252.0  2.126633e+06  4.552170e+06 -4.989856e+06   \n",
       "1    2020  356  51262.0  2.165382e+06  4.590845e+06 -4.937442e+06   \n",
       "2    2020  356  51272.0  2.203946e+06  4.628953e+06 -4.884473e+06   \n",
       "3    2020  356  51282.0  2.242319e+06  4.666488e+06 -4.830955e+06   \n",
       "4    2020  356  51292.0  2.280496e+06  4.703447e+06 -4.776894e+06   \n",
       "..    ...  ...      ...           ...           ...           ...   \n",
       "276  2020  356  54012.0 -2.069327e+06 -3.293189e+06  5.901384e+06   \n",
       "277  2020  356  54022.0 -2.120336e+06 -3.332440e+06  5.861198e+06   \n",
       "278  2020  356  54032.0 -2.171164e+06 -3.371243e+06  5.820351e+06   \n",
       "279  2020  356  54042.0 -2.221806e+06 -3.409594e+06  5.778847e+06   \n",
       "280  2020  356  54052.0 -2.272254e+06 -3.447490e+06  5.736690e+06   \n",
       "\n",
       "              vx           vy           vz       clock  \n",
       "0    3884.019377  3895.866320  5213.514095  30809652.0  \n",
       "1    3865.729372  3839.235712  5269.275977  30809662.0  \n",
       "2    3846.923435  3782.199871  5324.450320  30809672.0  \n",
       "3    3827.603002  3724.765828  5379.030847  30809682.0  \n",
       "4    3807.769575  3666.940659  5433.011344  30809692.0  \n",
       "..           ...          ...          ...         ...  \n",
       "276 -5109.703461 -3947.281735 -3985.399903  30812412.0  \n",
       "277 -5091.966582 -3902.738810 -4051.757205  30812422.0  \n",
       "278 -5073.593365 -3857.783983 -4117.659359  30812432.0  \n",
       "279 -5054.585233 -3812.423186 -4183.098903  30812442.0  \n",
       "280 -5034.943691 -3766.662398 -4248.068426  30812452.0  \n",
       "\n",
       "[281 rows x 10 columns]"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print(orb_ref.attrs)\n",
    "orb_ref"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "6ef04c21-c0dc-41df-a767-a7a46d38a682",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{'nd': '281', 'iy': '2021', 'id': '2', 'isec': '51252.000000', 'idsec': '10.000'}\n"
     ]
    },
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>iy</th>\n",
       "      <th>id</th>\n",
       "      <th>isec</th>\n",
       "      <th>px</th>\n",
       "      <th>py</th>\n",
       "      <th>pz</th>\n",
       "      <th>vx</th>\n",
       "      <th>vy</th>\n",
       "      <th>vz</th>\n",
       "      <th>clock</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>51252.0</td>\n",
       "      <td>2.128971e+06</td>\n",
       "      <td>4.554425e+06</td>\n",
       "      <td>-4.986805e+06</td>\n",
       "      <td>3882.942455</td>\n",
       "      <td>3892.555870</td>\n",
       "      <td>5216.801202</td>\n",
       "      <td>224052.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>51262.0</td>\n",
       "      <td>2.167709e+06</td>\n",
       "      <td>4.593067e+06</td>\n",
       "      <td>-4.934358e+06</td>\n",
       "      <td>3864.621540</td>\n",
       "      <td>3835.901754</td>\n",
       "      <td>5272.528762</td>\n",
       "      <td>224062.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>51272.0</td>\n",
       "      <td>2.206262e+06</td>\n",
       "      <td>4.631141e+06</td>\n",
       "      <td>-4.881357e+06</td>\n",
       "      <td>3845.784778</td>\n",
       "      <td>3778.842819</td>\n",
       "      <td>5327.668412</td>\n",
       "      <td>224072.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>51282.0</td>\n",
       "      <td>2.244623e+06</td>\n",
       "      <td>4.668643e+06</td>\n",
       "      <td>-4.827807e+06</td>\n",
       "      <td>3826.433611</td>\n",
       "      <td>3721.386098</td>\n",
       "      <td>5382.213880</td>\n",
       "      <td>224082.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>4</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>51292.0</td>\n",
       "      <td>2.282789e+06</td>\n",
       "      <td>4.705568e+06</td>\n",
       "      <td>-4.773714e+06</td>\n",
       "      <td>3806.569545</td>\n",
       "      <td>3663.538671</td>\n",
       "      <td>5436.158955</td>\n",
       "      <td>224092.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>...</th>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "      <td>...</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>276</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>54012.0</td>\n",
       "      <td>-2.072360e+06</td>\n",
       "      <td>-3.295447e+06</td>\n",
       "      <td>5.899060e+06</td>\n",
       "      <td>-5108.682789</td>\n",
       "      <td>-3944.684131</td>\n",
       "      <td>-3989.299038</td>\n",
       "      <td>226812.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>277</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>54022.0</td>\n",
       "      <td>-2.123359e+06</td>\n",
       "      <td>-3.334672e+06</td>\n",
       "      <td>5.858835e+06</td>\n",
       "      <td>-5090.908112</td>\n",
       "      <td>-3900.117442</td>\n",
       "      <td>-4055.630120</td>\n",
       "      <td>226822.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>278</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>54032.0</td>\n",
       "      <td>-2.174176e+06</td>\n",
       "      <td>-3.373448e+06</td>\n",
       "      <td>5.817949e+06</td>\n",
       "      <td>-5072.497178</td>\n",
       "      <td>-3855.139196</td>\n",
       "      <td>-4121.505617</td>\n",
       "      <td>226832.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>279</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>54042.0</td>\n",
       "      <td>-2.224807e+06</td>\n",
       "      <td>-3.411773e+06</td>\n",
       "      <td>5.776406e+06</td>\n",
       "      <td>-5053.451414</td>\n",
       "      <td>-3809.755328</td>\n",
       "      <td>-4186.918069</td>\n",
       "      <td>226842.0</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>280</th>\n",
       "      <td>2021</td>\n",
       "      <td>2</td>\n",
       "      <td>54052.0</td>\n",
       "      <td>-2.275243e+06</td>\n",
       "      <td>-3.449642e+06</td>\n",
       "      <td>5.734212e+06</td>\n",
       "      <td>-5033.772329</td>\n",
       "      <td>-3763.971820</td>\n",
       "      <td>-4251.860067</td>\n",
       "      <td>226852.0</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "<p>281 rows × 10 columns</p>\n",
       "</div>"
      ],
      "text/plain": [
       "       iy  id     isec            px            py            pz           vx  \\\n",
       "0    2021   2  51252.0  2.128971e+06  4.554425e+06 -4.986805e+06  3882.942455   \n",
       "1    2021   2  51262.0  2.167709e+06  4.593067e+06 -4.934358e+06  3864.621540   \n",
       "2    2021   2  51272.0  2.206262e+06  4.631141e+06 -4.881357e+06  3845.784778   \n",
       "3    2021   2  51282.0  2.244623e+06  4.668643e+06 -4.827807e+06  3826.433611   \n",
       "4    2021   2  51292.0  2.282789e+06  4.705568e+06 -4.773714e+06  3806.569545   \n",
       "..    ...  ..      ...           ...           ...           ...          ...   \n",
       "276  2021   2  54012.0 -2.072360e+06 -3.295447e+06  5.899060e+06 -5108.682789   \n",
       "277  2021   2  54022.0 -2.123359e+06 -3.334672e+06  5.858835e+06 -5090.908112   \n",
       "278  2021   2  54032.0 -2.174176e+06 -3.373448e+06  5.817949e+06 -5072.497178   \n",
       "279  2021   2  54042.0 -2.224807e+06 -3.411773e+06  5.776406e+06 -5053.451414   \n",
       "280  2021   2  54052.0 -2.275243e+06 -3.449642e+06  5.734212e+06 -5033.772329   \n",
       "\n",
       "              vy           vz     clock  \n",
       "0    3892.555870  5216.801202  224052.0  \n",
       "1    3835.901754  5272.528762  224062.0  \n",
       "2    3778.842819  5327.668412  224072.0  \n",
       "3    3721.386098  5382.213880  224082.0  \n",
       "4    3663.538671  5436.158955  224092.0  \n",
       "..           ...          ...       ...  \n",
       "276 -3944.684131 -3989.299038  226812.0  \n",
       "277 -3900.117442 -4055.630120  226822.0  \n",
       "278 -3855.139196 -4121.505617  226832.0  \n",
       "279 -3809.755328 -4186.918069  226842.0  \n",
       "280 -3763.971820 -4251.860067  226852.0  \n",
       "\n",
       "[281 rows x 10 columns]"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "print(orb_rep.attrs)\n",
    "orb_rep"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "968302f1-9586-441e-ae45-8127f82c4961",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([0., 0., 0.])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#reference orbit first point:  2126632.873229, 4552169.567944, -4989856.255835\n",
    "#orb[['px', 'py', 'pz']].head(1).values\n",
    "spline_ref = CubicHermiteSpline(orb_ref['clock'], orb_ref[['px', 'py', 'pz']].values, orb_ref[['vx', 'vy', 'vz']].values)\n",
    "spline_rep = CubicHermiteSpline(orb_rep['clock'], orb_rep[['px', 'py', 'pz']].values, orb_rep[['vx', 'vy', 'vz']].values)\n",
    "\n",
    "spline_ref(orb_ref['clock'].values[0]) - np.array([ 2126632.873229,  4552169.567944, -4989856.255835])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "e5585c3f-49e8-4f23-970b-1eaf92af454e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-8.16844404e-05, -5.17568551e-05, -7.23702833e-05])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# reference orbit start point:  3934597.486830, 3828232.879869, 4455902.914510\n",
    "spline_ref(t_ref[0]) - np.array([3934597.486830, 3828232.879869, 4455902.914510])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "2a467f89-ec3f-43bf-8f93-ea8d1fd28192",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-1.23162754e-05, -7.54557550e-06, -1.10631809e-05])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# reference orbit center point:    3931447.623914, 3821459.815849, 4464465.289022\n",
    "spline_ref(t_ref[1]) - np.array([3931447.623914, 3821459.815849, 4464465.289022])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "e1e793ab-99db-4aae-8596-26c2d0f1572d",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([-1.37868337e-05, -8.89413059e-06, -1.25141814e-05])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#reference orbit end point: 3928286.413630, 3814677.847949, 4473016.374198\n",
    "spline_ref(t_ref[2]) - np.array([3928286.413630, 3814677.847949, 4473016.374198])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "24a679d4-9c8e-400e-a202-55e5f24bb92c",
   "metadata": {},
   "source": [
    "### Repeat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "9e481337-b410-45aa-a7ef-7fef1150dc96",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(5.352194420993328e-08, 4.0832674130797386e-08, 2.814340405166149e-08)"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#repeat orbit times: 225449.207167, 225452.200057, 225450.703612\n",
    "t_rep = prm_rep.get_seconds()\n",
    "t_rep = [t_rep[0], np.sum(t_rep)/2, t_rep[-1]]\n",
    "t_rep[0] - 225449.207167, t_rep[1] - 225450.703612, t_rep[2] - 225452.200057"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "90126bc7-caa1-4b01-b7d7-17c018cd7b95",
   "metadata": {},
   "source": [
    "## TODO"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c62f15d-0d0b-432e-a2c4-fb3e4c546d09",
   "metadata": {},
   "outputs": [],
   "source": [
    "# void calc_height_velocity(\n",
    "#     struct SAT_ORB *orb,    // Pointer to a structure containing satellite orbit information.\n",
    "#     struct PRM *prm,        // Pointer to a structure containing parameters for processing.\n",
    "#     double t1,              // Start time for the calculations.\n",
    "#     double t2,              // End time for the calculations.\n",
    "#     double *height,         // Pointer to a double where the calculated height will be stored.\n",
    "#     double *re2,            // Pointer to a double where the calculated Earth's radius will be stored.\n",
    "#     double *vg,             // Pointer to a double where the calculated ground velocity will be stored.\n",
    "#     double *vtot,           // Pointer to a double where the calculated total velocity will be stored.\n",
    "#     double *rdot            // Pointer to a double where the calculated range rate will be stored.\n",
    "# );"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca4198a1-1abb-4398-9344-d9b0b5e6efcf",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "acec6b4c-ab7e-42d7-a459-424f431e5397",
   "metadata": {},
   "source": [
    "## SAT_baseline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "0570759d-18f0-4f2f-9f3d-d58d7674e022",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "B_parallel = 0.1531673895604653, B_perpendicular = 9.126210710508152\n",
      "Baseline at start:  total 9.127495942580225, alpha = 38.06073725529894\n",
      "Baseline at center: total 9.206160182341616, alpha = 37.6178342369064\n",
      "Baseline at end:    total 9.285371571510327, alpha = 37.18263652774751\n",
      "B_offset_start = -4.9088783493327055, B_offset_center = -4.887321379749086, B_offset_end = -4.865816040633634\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(4.0204629465301345e-05,\n",
       " -1.899094384860689e-05,\n",
       " -1.831369077542888e-05,\n",
       " -1.4724418383238458e-05,\n",
       " 8.868617326740491e-06,\n",
       " 4.371935294678053e-06,\n",
       " 1.6654989138942256e-06,\n",
       " -6.245616335220916e-07,\n",
       " -0.00025432195606356345,\n",
       " -0.0001948661066037971,\n",
       " 0.00010284119750991749)"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Interpolating positions for reference orbit\n",
    "pos_ref = spline_ref(t_ref)\n",
    "# Interpolating positions for repeat orbit\n",
    "pos_rep = spline_rep(t_rep)\n",
    "\n",
    "# Compute the initial distances\n",
    "def compute_initial_distances(prm, spline, t_ref, ref_pos):\n",
    "    ns2 = int(prm.get('nrows') * 0.5)\n",
    "    dt = 0.5 / prm.get('PRF')\n",
    "    times = t_ref + dt * range(-ns2, ns2)\n",
    "    #print ('compute_initial_distances times:', len(times), times)\n",
    "    positions = spline(times)\n",
    "    dists = np.linalg.norm(positions - ref_pos, axis=1)\n",
    "    idx = np.argmin(dists)\n",
    "    return times[idx], positions[idx], dists[idx]\n",
    "\n",
    "# Polynomial refinement\n",
    "def poly_interp(spline, t_ref, ref_pos):\n",
    "    ntt = 100\n",
    "    ddt = 0.01 / ntt\n",
    "    times = np.array([(k - ntt / 2 + 0.5) * ddt for k in range(ntt)])\n",
    "    #print ('def poly_interp(spline, t_ref, ref_pos): times:', len(times), times[:3], '...', times[-3:])\n",
    "    positions = spline(t_ref + times)\n",
    "    distances = np.linalg.norm(positions - ref_pos, axis=1)**2\n",
    "    p = np.polyfit(times, distances, 2)\n",
    "    t = t_ref - p[1] / (2.0 * p[0])\n",
    "    b = np.sqrt(p[2] - p[1] ** 2 / (4.0 * p[0]))\n",
    "    return t, spline(t), b\n",
    "\n",
    "ht = []\n",
    "B_offset = []\n",
    "baseline = []\n",
    "alpha = []\n",
    "for idx in range(3):\n",
    "    height, re_c = prm_rep.get_height(*spline_rep(t_rep[idx]))\n",
    "    ht.append(height + re_c - prm_rep.get('earth_radius'))\n",
    "    # find nearest points in the repeat orbit for the start, center, and end of the reference orbit\n",
    "    ts, pos_rep = compute_initial_distances(prm_ref, spline_rep, t_rep[idx], pos_ref[idx])[:2]\n",
    "    # refine baseline computation\n",
    "    ts, pos_rep = poly_interp(spline_rep, ts, pos_ref[idx])[:2]\n",
    "    B_offset.append( (ts - t_rep[idx]) * prm_rep.get('SC_vel') )\n",
    "    # baseline components\n",
    "    baseline.append( pos_rep - pos_ref[idx] )\n",
    "    bv, bh = prm_ref.get_components(baseline[idx],  pos_ref[idx], pos_rep)\n",
    "    alpha.append( np.degrees(np.arctan2(bv, bh)) )\n",
    "\n",
    "bpara, bperp = prm_ref.get_baseline_projections(prm_rep, baseline[0], alpha[0])\n",
    "\n",
    "# TODO: tie_point, look vectors, shifts\n",
    "# find expected offset in pixels (rshift and yshift)\n",
    "\n",
    "print(f\"B_parallel = {bpara}, B_perpendicular = {bperp}\")\n",
    "print(f\"Baseline at start:  total {np.linalg.norm(baseline[0])}, alpha = {alpha[0]}\")\n",
    "print(f\"Baseline at center: total {np.linalg.norm(baseline[1])}, alpha = {alpha[1]}\")\n",
    "print(f\"Baseline at end:    total {np.linalg.norm(baseline[2])}, alpha = {alpha[2]}\")\n",
    "print(f\"B_offset_start = {B_offset[0]}, B_offset_center = {B_offset[1]}, B_offset_end = {B_offset[2]}\")\n",
    "# B_parallel          = 0.153127184931 \n",
    "# B_perpendicular     = 9.126229701452 \n",
    "# baseline_start      = 9.127514256271 \n",
    "# baseline_center     = 9.206174906760 \n",
    "# baseline_end        = 9.285362702893 \n",
    "# alpha_start         = 38.060991577255 \n",
    "# alpha_center        = 37.618029103013 \n",
    "# alpha_end           = 37.182533686550 \n",
    "# B_offset_start      = -4.908882721268 \n",
    "# B_offset_center     = -4.887323045248 \n",
    "# B_offset_end        = -4.865815416072 \n",
    "\n",
    "#repeat orbit (ts - t21), vel: -0.000686, 7153.263621\n",
    "\n",
    "ht[2] - 700857.378603, ht[1] - 700872.176433, ht[0] - 700886.984341,\n",
    "bpara - 0.153127184931, bperp - 9.126229701452, \\\n",
    "np.linalg.norm(baseline[0])  - 9.127514256271, \\\n",
    "np.linalg.norm(baseline[1]) - 9.206174906760, \\\n",
    "np.linalg.norm(baseline[2])    - 9.285362702893, \\\n",
    "B_offset[0] - -4.908882721268, B_offset[1] - -4.887323045248, B_offset[2] - -4.865815416072, \\\n",
    "alpha[0] - 38.060991577255, alpha[1] - 37.618029103013, alpha[2] - 37.182533686550"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "79fcaad8-ab88-4d38-a4cf-5f922e66977a",
   "metadata": {},
   "outputs": [],
   "source": [
    "import scipy\n",
    "import numpy as np\n",
    "#from scipy.constants import speed_of_light\n",
    "\n",
    "from pyproj import Proj, Transformer\n",
    "\n",
    "# Initialize WGS84 coordinate system\n",
    "wgs84 = Proj(proj='latlong', datum='WGS84')\n",
    "wgs84_geocentric = Proj(proj='geocent', datum='WGS84')\n",
    "\n",
    "# Initialize the Transformer\n",
    "transformer_to_geocentric = Transformer.from_proj(wgs84, wgs84_geocentric)\n",
    "transformer_to_geodetic = Transformer.from_proj(wgs84_geocentric, wgs84)\n",
    "\n",
    "# Function to convert from ellipsoidal to Cartesian coordinates (plh to xyz)\n",
    "def plh2xyz(lat, lon, h):\n",
    "    x, y, z = transformer_to_geocentric.transform(lon, lat, h)\n",
    "    return np.array([x, y, z])\n",
    "\n",
    "# Function to convert from Cartesian to ellipsoidal coordinates (xyz to plh)\n",
    "def xyz2plh(x, y, z):\n",
    "    lon, lat, h = transformer_to_geodetic.transform(x, y, z)\n",
    "    return lat, lon, h"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "878bfcaa-ec99-45e0-bbc3-419d5952884d",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "lon_tie_point = 49.8734654788201\n",
      "lat_tie_point = 40.28637916813154\n",
      "4.788200982375201e-07 1.6813154246619888e-07\n"
     ]
    }
   ],
   "source": [
    "# Function to compute unit vectors\n",
    "def compute_unit_vectors(position):\n",
    "    ru = np.linalg.norm(position)\n",
    "    xu = position[0] / ru\n",
    "    yu = position[1] / ru\n",
    "    zu = position[2] / ru\n",
    "    return ru, xu, yu, zu\n",
    "\n",
    "# Calculate tie point and look vectors\n",
    "def calculate_tie_point_and_look_vectors(prm_ref, prm_rep, t_sta, t_vel, ref_pos_start, spline_ref):\n",
    "    # Get the necessary parameters from PRM\n",
    "    #earth_radius = prm_ref.get('earth_radius')\n",
    "    #near_range = prm_ref.get('near_range')\n",
    "    #ht = prm_ref.get('SC_height')\n",
    "    dr = 0.5 * scipy.constants.speed_of_light / prm_ref.get('rng_samp_rate')\n",
    "    far_range = prm_ref.get('near_range') + dr * prm_ref.get('num_rng_bins')\n",
    "    rho = (prm_ref.get('near_range') + far_range) / 2\n",
    "    \n",
    "    # Radar coordinates\n",
    "    b = prm_ref.get('SC_height') + prm_ref.get('earth_radius')\n",
    "    theta = np.arccos((b**2 + rho**2 - prm_ref.get('earth_radius')**2) / 2 / b / rho)\n",
    "    #print ('theta', theta)\n",
    "\n",
    "    radar_look = np.zeros(3)\n",
    "    radar_look[1] = np.cos(theta)\n",
    "    radar_look[2] = -np.sin(theta)\n",
    "    if prm_ref.get('lookdir') == 'L':\n",
    "        radar_look[2] = np.sin(theta)\n",
    "    \n",
    "    # Get the satellite position and velocity at t_sta and t_vel\n",
    "    x_sta, y_sta, z_sta = spline_ref(t_sta)\n",
    "    x_vel, y_vel, z_vel = spline_ref(t_vel)\n",
    "    \n",
    "    # Compute unit vectors\n",
    "    ru_sta, xu_sta, yu_sta, zu_sta = compute_unit_vectors(np.array([x_sta, y_sta, z_sta]))\n",
    "    ru_vel, xu_vel, yu_vel, zu_vel = compute_unit_vectors(np.array([x_vel - x_sta, y_vel - y_sta, z_vel - z_sta]))\n",
    "    \n",
    "    o1y = np.array([-xu_sta, -yu_sta, -zu_sta])\n",
    "    o1x = np.array([xu_vel, yu_vel, zu_vel])\n",
    "    \n",
    "    o1z = np.cross(o1x, o1y)\n",
    "    \n",
    "    # Look vector in global cartesian coordinates\n",
    "    glob_look = radar_look[0] * o1x + radar_look[1] * o1y + radar_look[2] * o1z\n",
    "    \n",
    "    # Compute the target in global cartesian coordinates\n",
    "    target = np.array([x_sta, y_sta, z_sta]) + glob_look * rho\n",
    "    \n",
    "    # Convert to ellipsoidal coordinates\n",
    "    lat, lon, h = xyz2plh(target[0], target[1], target[2])\n",
    "    target_llt = np.array([lat, lon, h])\n",
    "    \n",
    "    return target, target_llt\n",
    "\n",
    "# Determine tie point, look vectors, shifts\n",
    "t_sta = t_ref[0] + 2.0\n",
    "t_vel = t_ref[0] + 2.1\n",
    "#print ('t_sta', t_sta, 't_vel', t_vel)\n",
    "\n",
    "target, target_llt = calculate_tie_point_and_look_vectors(prm_ref, prm_rep, t_sta, t_vel, pos_ref[0], spline_ref)\n",
    "lon_tie_point = (target_llt[1] > 180.0) * (target_llt[1] - 360.0) + (target_llt[1] <= 180.0) * target_llt[1]\n",
    "lat_tie_point = target_llt[0]\n",
    "\n",
    "print(f\"lon_tie_point = {lon_tie_point}\")\n",
    "print(f\"lat_tie_point = {lat_tie_point}\")\n",
    "\n",
    "print (target_llt[1] - 49.873465, target_llt[0] - 40.286379)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "eff6064b-32b0-4337-9b64-a25c4b985394",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ashift = 0, rshift = 0\n"
     ]
    }
   ],
   "source": [
    "# Function to convert latitude, longitude, and height to range, azimuth, and height\n",
    "def llt2rat_sub(prm, spline, target_llt):\n",
    "    vel = prm.get('SC_vel')\n",
    "\n",
    "    # Convert target lat/lon/height to xyz (xp variable)\n",
    "    target_xyz = plh2xyz(target_llt[0], target_llt[1], target_llt[2])\n",
    "\n",
    "    # Compute start and stop time\n",
    "    t1 = 86400.0 * prm.get('clock_start') + (prm.get('nrows') - prm.get('num_valid_az')) / (2.0 * prm.get('PRF'))\n",
    "    t2 = t1 + prm.get('num_patches') * prm.get('num_valid_az') / prm.get('PRF')\n",
    "    # Sampling the orbit every 2nd point\n",
    "    ts = 2.0 / prm.get('PRF')\n",
    "    nrec = int((t2 - t1) / ts)\n",
    "\n",
    "    # orbit positions\n",
    "    # /* number of buffer points to add before and after the acquisition */\n",
    "    npad = 8000\n",
    "    orb_time = t1 - ts * npad + ts * range(nrec + npad * 2);\n",
    "    #print ('orb_time', len(orb_time))    \n",
    "    orb_pos = spline(orb_time)\n",
    "\n",
    "    dists = np.linalg.norm(orb_pos - target_xyz, axis=1)\n",
    "    idx = np.argmin(dists)\n",
    "    rng = dists[idx]\n",
    "    tm = orb_time[idx]\n",
    "\n",
    "    # Compute range and azimuth in pixel space and correct for azimuth bias\n",
    "    dr = 0.5 * scipy.constants.speed_of_light / prm.get('rng_samp_rate')\n",
    "    rng_pix = (rng - prm.get('near_range')) / dr - (prm.get('rshift') + prm.get('sub_int_r')) + prm.get('chirp_ext')\n",
    "    az_pix = prm.get('PRF') * (tm - t1) - (prm.get('ashift') + prm.get('sub_int_a'))\n",
    "\n",
    "    # Azimuth and range correction if Doppler is not zero\n",
    "    if prm.get('fd1') != 0.0:\n",
    "        rdd = (vel * vel) / rng\n",
    "        daa = -0.5 * (prm.get('radar_wavelength') * fd1) / rdd\n",
    "        drr = 0.5 * rdd * daa * daa / dr\n",
    "        daa = prf * daa\n",
    "        rng_pix += drr\n",
    "        az_pix += daa\n",
    "\n",
    "    target_rat = np.array([rng_pix, az_pix, np.linalg.norm(target_xyz) - prm.get('earth_radius')])\n",
    "    return target_rat\n",
    "\n",
    "target_rat_ref = llt2rat_sub(prm_ref, spline_ref, target_llt)\n",
    "target_rat_rep = llt2rat_sub(prm_rep, spline_rep, target_llt)\n",
    "#print ('target_rat_ref', target_rat_ref)\n",
    "#print ('target_rat_rep', target_rat_rep)\n",
    "\n",
    "# # Compute expected offset in pixels (rshift and yshift)\n",
    "ashift = int(target_rat_rep[1] - target_rat_ref[1])\n",
    "rshift = int(target_rat_rep[0] - target_rat_ref[0])\n",
    "\n",
    "print(f\"ashift = {ashift}, rshift = {rshift}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "1d8e5447-7b44-4361-a75f-9dc0f1e0fcc2",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.00016314880667778198,\n",
       " 2.0000007996870863,\n",
       " -2.0972573162580943e-06,\n",
       " -0.0001307832390011754,\n",
       " -2.0000000056288627,\n",
       " -2.0972573162580943e-06)"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#llt2rat_sub target_rat:  12794.831537, 1038.380102, -5.071846\n",
    "#llt2rat_sub target_rat:  12794.614100, 1041.773079, -5.071846\n",
    "target_rat_ref[0] - 12794.831537, target_rat_ref[1] - 1038.380102, target_rat_ref[2] - -5.071846, \\\n",
    "target_rat_rep[0] - 12794.614100, target_rat_rep[1] - 1041.773079, target_rat_rep[2] - -5.071846"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "74b17c13-bd06-420e-8f24-15458bd8fa74",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "Object PRM 22 items\n",
       "                         value\n",
       "name                          \n",
       "lon_tie_point        49.873465\n",
       "lat_tie_point        40.286379\n",
       "SC_vel                    TODO\n",
       "SC_height        700872.176382\n",
       "SC_height_start  700886.984179\n",
       "SC_height_end    700857.378602\n",
       "earth_radius     6369585.03852\n",
       "rshift                       0\n",
       "sub_int_r                  0.0\n",
       "ashift                       0\n",
       "sub_int_a                  0.0\n",
       "B_parallel            0.153167\n",
       "B_perpendicular       9.126211\n",
       "baseline_start        9.127496\n",
       "baseline_center        9.20616\n",
       "baseline_end          9.285372\n",
       "alpha_start          38.060737\n",
       "alpha_center         37.617834\n",
       "alpha_end            37.182637\n",
       "B_offset_start       -4.908878\n",
       "B_offset_center      -4.887321\n",
       "B_offset_end         -4.865816"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "PRM().set(\n",
    "    lon_tie_point=lon_tie_point,\n",
    "    lat_tie_point=lat_tie_point,\n",
    "    SC_vel='TODO',\n",
    "    SC_height=ht[1],\n",
    "    SC_height_start=ht[0],\n",
    "    SC_height_end=ht[2],\n",
    "    earth_radius=prm_rep.get('earth_radius'),\n",
    "    rshift=rshift,   \n",
    "    sub_int_r=0.0,\n",
    "    ashift=ashift,\n",
    "    sub_int_a=0.0,\n",
    "    B_parallel=bpara,\n",
    "    B_perpendicular=bperp,\n",
    "    baseline_start=np.linalg.norm(baseline[0]),\n",
    "    baseline_center=np.linalg.norm(baseline[1]),\n",
    "    baseline_end=np.linalg.norm(baseline[2]),\n",
    "    alpha_start=alpha[0],\n",
    "    alpha_center=alpha[1],\n",
    "    alpha_end=alpha[2],\n",
    "    B_offset_start=B_offset[0],\n",
    "    B_offset_center=B_offset[1],\n",
    "    B_offset_end=B_offset[2]\n",
    ")\n",
    "# lon_tie_point =  49.873465\n",
    "# lat_tie_point =  40.286379\n",
    "# SC_vel              = 7153.263621000000 \n",
    "# SC_height           = 700872.176433234476 \n",
    "# SC_height_start     = 700886.984341199510 \n",
    "# SC_height_end       = 700857.378603472374 \n",
    "# earth_radius        = 6369585.038519999944 \n",
    "# rshift              = 0 \n",
    "# sub_int_r           = 0.0 \n",
    "# ashift              = 3\n",
    "# sub_int_a           = 0.0 \n",
    "# B_parallel          = 0.153127184931 \n",
    "# B_perpendicular     = 9.126229701452 \n",
    "# baseline_start      = 9.127514256271 \n",
    "# baseline_center     = 9.206174906760 \n",
    "# baseline_end        = 9.285362702893 \n",
    "# alpha_start         = 38.060991577255 \n",
    "# alpha_center        = 37.618029103013 \n",
    "# alpha_end           = 37.182533686550 \n",
    "# B_offset_start      = -4.908882721268 \n",
    "# B_offset_center     = -4.887323045248 \n",
    "# B_offset_end        = -4.865815416072 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "8d7c3927-c7a6-40fd-842c-e7d29755c26e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "519 ns ± 4.76 ns per loop (mean ± std. dev. of 7 runs, 1,000,000 loops each)\n"
     ]
    }
   ],
   "source": [
    "%timeit transformer_to_geocentric.transform(0, 0, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 91,
   "id": "f7f3e8e8-6773-49a2-8a5a-4b4edc3eacc3",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "vtot 7593.081557320316\n",
      "rs 7070457.214901702 height 700872.1199148409 re2 6369585.094986861\n",
      "rlat 0.6979446746557342 re 6369279.170489837\n",
      "time [-20.55556301 -20.14445175 -19.73334049]\n",
      "rng [12943.19632494 12437.67610874 11942.01055035]\n",
      "d [30.26037086 -8.31407475  8.11243721]\n",
      "rdot -8.314074746997731 vg 7153.264204991109\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(700872.1199148409, 6369585.094986861, 7153.264204991109)"
      ]
     },
     "execution_count": 91,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "\n",
    "def get_velocity(self, spline, t1, t2):\n",
    "    # ellipsoid parameters\n",
    "    ra = self.get('equatorial_radius')\n",
    "    rc = self.get('polar_radius')\n",
    "\n",
    "    dt = 200.0 / self.get('PRF')\n",
    "    # nt = (prm->nrows - prm->num_valid_az)/100.\n",
    "\t# changed to fixed value for TSX, ENVI\n",
    "    nt = 100\n",
    "\n",
    "    t0 = (t1 + t2) / 2.0\n",
    "    t1 = t0 - 2.0\n",
    "    t2 = t0 + 2.0\n",
    "\n",
    "    xs, ys, zs = spline(t0)\n",
    "    x1, y1, z1 = spline(t1)\n",
    "    x2, y2, z2 = spline(t2)\n",
    "    #print ('xs, ys, zs', xs, ys, zs)\n",
    "    \n",
    "    vx = (x2 - x1) / 4.0\n",
    "    vy = (y2 - y1) / 4.0\n",
    "    vz = (z2 - z1) / 4.0\n",
    "    vs = np.sqrt(vx**2 + vy**2 + vz**2)\n",
    "    print ('vtot', vs)\n",
    "\n",
    "    orbdir = 'A' if vz > 0 else 'D'\n",
    "\n",
    "    rs = np.sqrt(xs**2 + ys**2 + zs**2)\n",
    "    height, re = self.get_height(xs, ys, zs)\n",
    "    re2 = re\n",
    "    print ('rs', rs, 'height', height, 're2', re2)\n",
    "    \n",
    "    a = np.array([xs / rs, ys / rs, zs / rs])\n",
    "    b = np.array([vx / vs, vy / vs, vz / vs])\n",
    "    c = np.cross(a, b)\n",
    "    #print ('c', c)\n",
    "\n",
    "    ro = self.get('near_range')\n",
    "    ct = (rs**2 + ro**2 - re**2) / (2.0 * rs * ro)\n",
    "    st = np.sin(np.arccos(ct))\n",
    "\n",
    "    xe = xs + ro * (-st * c[0] - ct * a[0])\n",
    "    ye = ys + ro * (-st * c[1] - ct * a[1])\n",
    "    ze = zs + ro * (-st * c[2] - ct * a[2])\n",
    "    rlat = np.arcsin(ze / re)\n",
    "    st = np.sin(rlat)\n",
    "    ct = np.cos(rlat)\n",
    "    arg = (ct**2) / (ra**2) + (st**2) / (rc**2)\n",
    "    re = 1.0 / np.sqrt(arg)\n",
    "\n",
    "    print ('rlat', rlat, 're', re)\n",
    "    \n",
    "    #time = np.linspace(-dt * (nt // 2), dt * (nt // 2), nt) + t0\n",
    "    time = np.zeros(nt)\n",
    "    rng = np.zeros(nt)\n",
    "    for k in range(nt):\n",
    "        t = dt * (k - nt / 2)\n",
    "        xs, ys, zs = spline(t0 + t)\n",
    "        time[k] = t\n",
    "        rng[k] = np.sqrt((xe - xs)**2 + (ye - ys)**2 + (ze - zs)**2) - ro\n",
    "        #print ('\\t', t, rng[k])\n",
    "    print ('time', time[:3])\n",
    "    print ('rng', rng[:3])\n",
    "    \n",
    "    d = np.polyfit(time, rng, 2)\n",
    "    print ('d', d)\n",
    "    rdot = d[1]\n",
    "    vg = np.sqrt(ro * 2.0 * d[0])\n",
    "\n",
    "    print ('rdot', rdot, 'vg', vg)\n",
    "    \n",
    "    return height, re2, vg, vs, rdot\n",
    "\n",
    "PRM.get_velocity=get_velocity\n",
    "height, re, vel,_,_ = prm_rep.get_velocity(spline_rep, t_rep[0], t_rep[2])\n",
    "#SC_vel              = 7153.263621000000 \n",
    "#SC_height           = 700872.176433234476 \n",
    "\n",
    "#====calc_height_velocity rs, height, re2: 7070457.214953, 700872.119966, 6369585.094987\n",
    "#====calc_height_velocity c: 0.783642, -0.594089, -0.181556\n",
    "#====calc_height_velocity rlat: 0.697945\n",
    "#====calc_height_velocity re: 6369279.170492\n",
    "#====calc_height_velocity nt: 100\n",
    "# \t\tt1, rng: 225430.148049, 12943.196271\n",
    "# \t\tt1, rng: 225430.559160, 12437.676028\n",
    "# \t\tt1, rng: 225430.970272, 11942.010446\n",
    "# ...\n",
    "# \t\tt1, rng: 225470.025841, 11134.385262\n",
    "# \t\tt1, rng: 225470.436953, 11613.425381\n",
    "# \t\tt1, rng: 225470.848064, 12102.343004\n",
    "#====time: -20.555563 -20.144452 -19.733340...\n",
    "#====rng: 12943.196271 12437.676028 11942.010446 ...\n",
    "#====d: 8.112514 -8.314069 30.260371\n",
    "#====rdot, vg: -8.314069 7153.264199\n",
    "height, re, vel\n",
    "\n",
    "#vtot 7593.081557320316"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 83,
   "id": "1bc3d83b-ea24-4b8f-b778-a8b41ee91925",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "xs, ys, zs 3931418.56659841 3821410.3406288857 4464541.928411318\n",
      "rs 7070457.097515175 height 700872.2622497436 re2 6369584.835265432\n",
      "c [ 0.7836413  -0.59408937 -0.18155562]\n",
      "rlat 0.6979571029166931 re 6369278.90854201\n",
      "time [-20.55556301 -20.14445175 -19.73334049]\n",
      "rng [12943.19337763 12437.67325908 11942.00779676]\n",
      "d [30.26036597 -8.31403128  8.11243604]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "(700872.2622497436,\n",
       " 6369584.835265432,\n",
       " 7153.263627278231,\n",
       " 7593.081349211761,\n",
       " -8.314031284402429)"
      ]
     },
     "execution_count": 83,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "t1 = (86400.0) * prm_rep.get('clock_start') + (prm_rep.get('nrows') - prm_rep.get('num_valid_az')) / (2.0 * prm_rep.get('PRF'))\n",
    "t2 = t1 + prm_rep.get('num_patches') * prm_rep.get('num_valid_az') / prm_rep.get('PRF')\n",
    "t1, t2\n",
    "prm_rep.get_velocity(spline_rep, t1, t2)\n",
    "#SC_vel              = 7153.263621000000 \n",
    "#SC_height           = 700872.176433234476 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "id": "7a4f1215-5395-4e33-806a-54fe1c8dd2c0",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "SC_vel = 7593.081557320316\n",
      "SC_height = 7070151.290404678\n"
     ]
    }
   ],
   "source": [
    "def get_velocity(prm, spline, t1, t2):\n",
    "    ra = prm.get('equatorial_radius')\n",
    "    rc = prm.get('polar_radius')\n",
    "    dt = 200.0 / prm.get('PRF')\n",
    "    nt = 100\n",
    "\n",
    "    t0 = (t1 + t2) / 2.0\n",
    "    t1 = t0 - 2.0\n",
    "    t2 = t0 + 2.0\n",
    "\n",
    "    xs, ys, zs = spline(t0)\n",
    "    x1, y1, z1 = spline(t1)\n",
    "    x2, y2, z2 = spline(t2)\n",
    "\n",
    "    rs = np.sqrt(xs**2 + ys**2 + zs**2)\n",
    "    vx = (x2 - x1) / 4.0\n",
    "    vy = (y2 - y1) / 4.0\n",
    "    vz = (z2 - z1) / 4.0\n",
    "    vs = np.sqrt(vx**2 + vy**2 + vz**2)\n",
    "    vtot = vs\n",
    "\n",
    "    rlat = np.arcsin(zs / rs)\n",
    "    arg = np.cos(rlat)**2 / ra**2 + np.sin(rlat)**2 / rc**2\n",
    "    re = 1.0 / np.sqrt(arg)\n",
    "    height = rs - re\n",
    "\n",
    "    a = np.array([xs / rs, ys / rs, zs / rs])\n",
    "    b = np.array([vx / vs, vy / vs, vz / vs])\n",
    "    c = np.cross(a, b)\n",
    "\n",
    "    ro = prm.get('near_range')\n",
    "    ct = (rs**2 + ro**2 - re**2) / (2.0 * rs * ro)\n",
    "    st = np.sin(np.arccos(ct))\n",
    "\n",
    "    xe = xs + ro * (-st * c[0] - ct * a[0])\n",
    "    ye = ys + ro * (-st * c[1] - ct * a[1])\n",
    "    ze = zs + ro * (-st * c[2] - ct * a[2])\n",
    "    rlat = np.arcsin(ze / re)\n",
    "    arg = (np.cos(rlat)**2) / (ra**2) + (np.sin(rlat)**2) / (rc**2)\n",
    "    re = 1.0 / np.sqrt(arg)\n",
    "\n",
    "    time = np.linspace(-dt * (nt // 2), dt * (nt // 2), nt) + t0\n",
    "    rng = np.zeros(nt)\n",
    "\n",
    "    for k in range(nt):\n",
    "        t = time[k]\n",
    "        xs, ys, zs = spline(t)\n",
    "        rng[k] = np.sqrt((xe - xs)**2 + (ye - ys)**2 + (ze - zs)**2) - ro\n",
    "\n",
    "    d = np.polyfit(time, rng, 2)\n",
    "    rdot = d[1]\n",
    "    vg = np.sqrt(ro * 2.0 * d[2])\n",
    "    return height, re, vg, vtot, rdot\n",
    "\n",
    "# Example usage\n",
    "params = {\n",
    "    'equatorial_radius': 6378137.0,\n",
    "    'polar_radius': 6356752.3,\n",
    "    'PRF': 1679.902,  # example value\n",
    "    'near_range': 850000.0  # example value\n",
    "}\n",
    "\n",
    "PRM.get_velocity=get_velocity\n",
    "\n",
    "height, re, vg, vtot, rdot = prm_rep.get_velocity(spline_rep, t_rep[0], t_rep[2])\n",
    "\n",
    "print(f\"SC_vel = {vtot}\")\n",
    "print(f\"SC_height = {height + re}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "id": "6ff7db33-3282-4fee-9e3c-4d24d3b4d659",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([0.411111, 0.411112])"
      ]
     },
     "execution_count": 76,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#====time: -20.555563 -20.144452 -19.733340...\n",
    "np.diff([-20.555563, -20.144452, -19.733340])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
